#ifdef GCC_MEGA64_AVR
	/* EEPROM routines used only with the WinAVR compiler. */
	#include <avr/eeprom.h> 
#endif
#include <avr/interrupt.h>
/* Scheduler include files. */
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"


//robo include files
#include "LCDtasks.h"
#include "pwm.h"
#include "define.h"
#include "FreeRTOS_speed.h"
#include "FreeRTOS_adc.h"
#include "FreeRTOS_pid.h"
#include "FreeRTOS_physical.h"
#include "FreeRTOS_encoder.h"
#include "FreeRTOS_forward.h"
//prototype include
#include "robotasks.h"



#define roboHANDLER_STACK_SIZE		( configMINIMAL_STACK_SIZE )
#define roboCONTROLLER_STACK_SIZE	( configMINIMAL_STACK_SIZE )//highest priority
#define roboQUEUE_LEN					2

/*we don't have to block to send. */
#define roboNO_BLOCK					( ( portTickType ) 0 )
/* The task will block on the queue for a long period. */
#define roboBLOCK_TIME					( ( portTickType ) 0xffff )

static void  vInitRobo(void);
static void vMainTask( void *pvParameters );
static void vWheelsTask( void *pvParameters );
static void vInputTask( void *pvParameters );
static void vDC3Task( void *pvParameters );

//static xQueueHandle xComQueue;
 	//static unsigned char ucParameterToPass;
static xSemaphoreHandle 	xEcdrSmphr = NULL;
static xSemaphoreHandle 	xADCSmphr  = NULL;
static xSemaphoreHandle	xDC3snsrSmphr= NULL;
static xSemaphoreHandle	xSwButSmphr= NULL;

static xSemaphoreHandle	xWheelsSmphr = NULL;
static xSemaphoreHandle	xDC3Smphr	= NULL;
static xSemaphoreHandle	xTmr4InputSmphr= NULL;
//xSemaphoreHandle	xTmr4Ecdr= NULL;
//Input
int L_ept_v,R_ept_v;//control by xWheelsTask
int stand_speed;//control by xMainTask
unsigned char hold_velocity_flag;//control by xWheelsTask
unsigned char update_flag;//control by xMainTask
//Switch, buttons
unsigned char inputBuf[4];//read by xMainTask and xDC3Task
//ADC: left/right_speed, crossline_flag
//input: stand_speed,update_flag;
//output:
int L_spd_buf,R_spd_buf;//read by xWheelsTask
unsigned char crossline_flag;//read by xWheelsTask
//Encoder: left/right_speed
//input: hold_velocity_flag,L_ept_v,R_ept_v
//output:
int L_spd_Ebuf,R_spd_Ebuf;//read by xWheelsTask
//DC3 Sensor
unsigned char DC3snsr_buf;//don't use
void vStartRoboTasks(unsigned portBASE_TYPE uxPriority){
	//static xTaskHandle xHandle_Ctl;
	//static xTaskHandle xHandle_Hnd;
	vInitRobo();
	vSemaphoreCreateBinary( xEcdrSmphr );
	vSemaphoreCreateBinary( xADCSmphr );
	vSemaphoreCreateBinary( xDC3snsrSmphr );
	vSemaphoreCreateBinary( xSwButSmphr );
	vSemaphoreCreateBinary( xTmr4InputSmphr );
	vSemaphoreCreateBinary( xWheelsSmphr );
	vSemaphoreCreateBinary(xDC3Smphr);
	
	xSemaphoreTake(xEcdrSmphr,0);
	xSemaphoreTake(xADCSmphr,0);
	xSemaphoreTake(xDC3snsrSmphr,0);
	xSemaphoreTake(xSwButSmphr,0);
	xSemaphoreTake(xWheelsSmphr,0);
	xSemaphoreTake(xDC3Smphr,0);
	//portENTER_CRITICAL();
	{
		/* Create the queues used for comunication between the Controller and Handler task. */
		//xComQueue = xQueueCreate( roboQUEUE_LEN, ( unsigned portBASE_TYPE ) sizeof( char *) );
	}
	//portEXIT_CRITICAL();
	//if(xComQueue != 0){
		xTaskCreate( vMainTask, ( signed char * ) "Main", roboCONTROLLER_STACK_SIZE,NULL, uxPriority-2, NULL);
		xTaskCreate( vWheelsTask, ( signed char * ) "wl", 100, NULL, uxPriority-1, NULL );
		xTaskCreate( vDC3Task, ( signed char * ) "DC3", 100,NULL, uxPriority-1, NULL );
		xTaskCreate( vInputTask, ( signed char * ) "In", 100,NULL, uxPriority, NULL);
		//xTaskCreate( vMainTask, ( signed char * ) "ctl", roboCONTROLLER_STACK_SIZE,NULL, uxPriority, NULL);

	//}
}
static void  vInitRobo(void){
	init_timer3();//PWM
	init_ADC();//ADC
	//IO of board
	PORTG=0x1f;
	DDRG=0x07;
	
	PORTA=0xFF;
	DDRA=0xFF;
	DDRD = 0xFF;
	
	OCR3AL=0x00;
	OCR3BL=0x00;
	OCR3CL=0x00;
	// External Interrupt(s) initialization
	// INT0: Off
	// INT1: Off
	// INT2: On
	// INT2 Mode: Rising Edge
	// INT3: On
	// INT3 Mode: Rising Edge
	// INT4: Off
	// INT5: Off
	// INT6: On
	// INT6 Mode: Rising Edge
	// INT7: On
	// INT7 Mode: Rising Edge
	EICRA=0xF0;
	EICRB=0xF0;
	EIMSK=0xCC;
	EIFR=0xCC;
	//interrupt OCP B
	//TCCR1B =0x0B;
	TCCR1A 	&=0xCC;
	TIMSK	|=0x08;
	//OCR1AH = 0xaD;
	//OCR1AL =0xFF;

}
static void vMainTask( void *pvParameters ){
	//Variables
	unsigned char _L1,_L2,_L3,_R1,_R2,_R3,start_flag = 0;
	int _speed_max=100,_speed_min=0;
	//Init variables
	update_flag = 0;
	/* Just to stop compiler warnings. */
	( void ) pvParameters;
	
	for(;;){
		xSemaphoreTake(xWheelsSmphr,0);
		xSemaphoreTake(xDC3Smphr,0);
		_L1 =  get_button(inputBuf[1],B_L1);
		_L2 =  get_button(inputBuf[1],B_L2);
		_L3 =  get_button(inputBuf[1],B_L3);
		_R1 =  get_button(inputBuf[1],B_R1);
		_R2 =  get_button(inputBuf[1],B_R2);
		_R3 =  get_button(inputBuf[1],B_R3);
		//test
		//inputBuf[0] = 1;
		//update_flag =0;
		//stand_speed = 30;
		switch(inputBuf[0]){
			case 0://mode 0: test update ADC sensor, stand_speed,
				if(_L1){
					update_flag = 1;
					xPrintStrLCD(16,"Up",0);
					xSemaphoreGive(xADCSmphr);
				}
				if (_L2)
				{
					faster_speed(&stand_speed,_speed_max);
				}
				else
				if (_L3)
				{
					slower_speed(&stand_speed,_speed_min);
				}
				break;
			case 1://mode 1: only xWheelsTask
				if(_L1) start_flag=1;
				if(_R1) start_flag=0;
				//test
				//start_flag=1;
				if(start_flag){
					xSemaphoreGive(xWheelsSmphr);
				}
				break;
			case 2://mode 2: only xDC3Task
				if(_L1) start_flag=1;
				if(_R1) start_flag=0;
				if(start_flag){
					xSemaphoreGive(xDC3Smphr);
				}
				break;
			case 3://mode 3: both
				if(_L1) start_flag=1;
				if(_R1) start_flag=0;
				if(start_flag){
					xSemaphoreGive(xDC3Smphr);
					xSemaphoreGive(xWheelsSmphr);
				}
				break;
		}
		vTaskDelay(100);
	}
}
static void vDC3Task( void *pvParameters ){
	//Variables
	
	/* Just to stop compiler warnings. */
	( void ) pvParameters;
	//Init variables
	
	for(;;){
		if(xSemaphoreTake( xDC3Smphr,0) == pdTRUE){
            // It is time to execute.
			xSemaphoreTake( xDC3snsrSmphr,0);
			
			if(get_sensor(inputBuf[2],B_SENSOR_HIGH)){ //kiem tra lai MASK
				dc3_speed(50);
			}else if(get_sensor(inputBuf[2],B_SENSOR_LOW)){
				dc3_speed(-50);
			}
			xSemaphoreGive(xDC3snsrSmphr);
			//
			xSemaphoreGive(xDC3Smphr);
        }else{
			dc3_speed(0);
		}
		vTaskDelay(10);
	}
}
static void vWheelsTask( void *pvParameters ){
	//Variables
	unsigned char function_forward_e = FWD_E_DONE,function_right=TURNRIGHT_NO;
	unsigned char turn_flag = 0,turnright_flag = 0;
	/* Just to stop compiler warnings. */
	( void ) pvParameters;
	//Init variables
	L_ept_v =0;
	R_ept_v =0;
	hold_velocity_flag =0;
	//test
	//crossline_flag = IS_LINE;
	for(;;){
		// Block waiting for the semaphore to become available.
        if(xSemaphoreTake( xWheelsSmphr,0) == pdTRUE){
            //It is time to execute.
			xSemaphoreTake(xEcdrSmphr,0);
			xSemaphoreTake(xADCSmphr,0);
			if(crossline_flag == CROSS_LINE){
				//forward before turn right
				function_forward_e = FWD_E_START;
				turn_flag = 1;
				xSemaphoreGive(xEcdrSmphr);
			}
			if(crossline_flag == IS_LINE && turn_flag != 1){
				speed(L_spd_buf,R_spd_buf);
				//test
				xPrintNumLCD(0,(long)L_spd_buf,3,0);
				xPrintNumLCD(6,(long)R_spd_buf,3,0);
				xSemaphoreGive(xADCSmphr);
			}
			if(turn_flag == 1){
				//preparing for turn right
				if(function_forward_e !=FWD_E_DONE)
					run_forward_vel_encoder(&function_forward_e,8000,
									&L_ept_v, &R_ept_v, &hold_velocity_flag,4);
				else{
					if(turnright_flag == 0){
						function_right = TURNRIGHT_START;
						turnright_flag = 1;
					}else{
						turnright_process(&function_right, &L_ept_v, &R_ept_v, &hold_velocity_flag);
						if(function_right== TURNRIGHT_DONE){
							function_right = TURNRIGHT_NO;
							turnright_flag = 0;
							turn_flag = 0;
						}
					}
				}
				speed(L_spd_Ebuf,R_spd_Ebuf);
				xSemaphoreGive(xEcdrSmphr);
			}else{
				xSemaphoreGive(xADCSmphr);
				speed(0,0);
			}
			//
			xSemaphoreGive(xWheelsSmphr);
        }else{
			speed(0,0);
		}
		vTaskDelay(10);
		
	}
}


static void vInputTask( void *pvParameters ){
	//Variables
	unsigned char _V_compare[8],ucSensors;
	int _dev=11, _old_dev=0,L_real_v,R_real_v;
	/* Just to stop compiler warnings. */
	( void ) pvParameters;
	//Init variables
	init_physical(&inputBuf[1],&inputBuf[2], &inputBuf[3], &inputBuf[0]);
	init_encoder();
	read_Vcompare(_V_compare);
	for(;;){
		// Block waiting for the semaphore to become available.
        if( xSemaphoreTake( xTmr4InputSmphr, roboBLOCK_TIME ) == pdTRUE )
        {
            // It is time to execute.
			//encoder
			if(xSemaphoreTake( xEcdrSmphr, 0) == pdTRUE ){
				//sample vantoc_L/R
				GET_VELOCITY(L_real_v,R_real_v);// ~ Sample_LR();
				if(hold_velocity_flag == 1)
					pd_velocity(L_ept_v,R_ept_v, L_real_v, R_real_v, &L_spd_Ebuf, &R_spd_Ebuf);
				//Give semaphor
				xSemaphoreGive(xEcdrSmphr);
			}
			//ADC
			if(xSemaphoreTake( xADCSmphr, 0) == pdTRUE ){
				if(update_flag == 0){
					//read sonsor, check cross line: crossline_flag
					read_sensor(&ucSensors,_V_compare);
					writeIO(OUTLED,ucSensors);
					check_crossline(ucSensors,&crossline_flag);// flag crossline
					xPrintNumLCD(16,(long)ucSensors,3,0);
					test_sensor(ucSensors, &_dev, &_old_dev);
					calc_forward_pd(_dev, &_old_dev,stand_speed,&L_spd_buf,&R_spd_buf);
				}else{
					save_sensor(BLACK,_V_compare);
					update_flag = 0;
				}
				//Give semaphor
				xSemaphoreGive(xADCSmphr);
			}
			//DC3 sensor
			if(xSemaphoreTake( xDC3snsrSmphr, 0) == pdTRUE ){
				//
				//DC3snsr_buf=GET_DC3SENSOR(inputBuf[3],B_SENSOR_HIGH);
				//Give semaphor
				xSemaphoreGive(xDC3snsrSmphr);
			}
        }
	}
}
static unsigned char ucInputTickCount = 0;
static unsigned char ucSBTickCount = 0;
void SIG_OUTPUT_COMPARE1B( void ) __attribute__ ( ( signal, naked ) );
void SIG_OUTPUT_COMPARE1B( void ){
	

	static signed portBASE_TYPE xHigherPriorityTaskWoken;

    // A timer tick has occurred.

    // ... Do other time functions.

    // Is it time for vATask () to run?
	xHigherPriorityTaskWoken = pdFALSE;
    ucInputTickCount++;
	ucSBTickCount++;
    if( ucInputTickCount >= 5 )//5 tick = 5ms
    {
        // Unblock the task by releasing the semaphore.
        xSemaphoreGiveFromISR( xTmr4InputSmphr, &xHigherPriorityTaskWoken );
		
        // Reset the count so we release the semaphore again in 10 ticks time.
        ucInputTickCount = 0;
    }
	if(ucSBTickCount >=10){//10 tick = 10ms
		//TODO sw button
		updateInput(&inputBuf[1],&inputBuf[2], &inputBuf[3], &inputBuf[0]);
		ucSBTickCount =0;
	}
    if( xHigherPriorityTaskWoken != pdFALSE )
    {
        // We can force a context switch here.  Context switching from an
        // ISR uses port specific syntax.  Check the demo task for your port
        // to find the syntax required.
		taskYIELD();
    }
	asm volatile ( "reti" );
}

